/*******************************************************************************
* This file implements the communication with the serial LCD.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "slcd.h"
#include "dcm.h"
#include "hal_adc.h"
#include "hal_gps.h"
#include "hal_bmp085.h"
#include "hal_sensors.h"
#include "airspeed.h"



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void prv_vClearScreen(void);
static void prv_vSetCursorPosition(unsigned char ucRow, unsigned char ucColumn);
static void prv_vSetBacklight(unsigned char ucBrightness);



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Buffer.
static char pcUartWriteBuffer[50];



/*******************************************************************************
* TASK: taskSLcd
*
* DESCRIPTIONS:
* This task sends the data to display on serial LCD
*
*******************************************************************************/
portTASK_FUNCTION(taskSLcd, pvParameters)
{
//	// Clear screen and set the backlight.
//	prv_vClearScreen();
//	prv_vSetBacklight(30);
	
	
	// We need to initialise xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	while (1) {
		
//		float fAirspeed = fGetAirspeed();
	//	if (fAirspeed > 100.0f) {
//			sprintf(pcUartWriteBuffer, "%u\r\n", uiReadAdc(0));
//			ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	//	}
		struct GPS_DATA xGpsData = xGetGpsData();
		struct ATTITUDE xAttitude = xGetAttitude();
		
//		sprintf(pcUartWriteBuffer, "Roll:%+06.1f Pitch:%+06.1f Yaw:%+06.1f\r\n",
//			    (double)xAttitude.fRoll * RAD_TO_DEG,
//			    (double)xAttitude.fPitch * RAD_TO_DEG,
//			    (double)xAttitude.fYaw * RAD_TO_DEG);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
//		
//		// Delay for 500ms (2Hz).
//		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 5);
//		
//		continue;
//		
		// First row.
		prv_vSetCursorPosition(0, 0);

//                struct MAG_DATA_RAW xMagDataRaw = xReadMagRaw();

//                struct ACC_DATA_RAW xAccDataRaw = xReadAccRaw();
		
		
		sprintf(pcUartWriteBuffer, "R%+06.1f P%+06.1f   ", (double)(xAttitude.fRoll * RAD_TO_DEG), (double)(xAttitude.fPitch * RAD_TO_DEG));
		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));

//                sprintf(pcUartWriteBuffer, "X%05u Y%05u   ", xMagDataRaw.siMagX, xMagDataRaw.siMagY);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));

//                sprintf(pcUartWriteBuffer, "X%+06d Y%+06d   ", xAccDataRaw.siAccX, xAccDataRaw.siAccY);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		
		
		
		// Second row.
		prv_vSetCursorPosition(1, 0);

//                sprintf(pcUartWriteBuffer, "Z%+06d          ", xAccDataRaw.siAccZ);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));

//                sprintf(pcUartWriteBuffer, "Z%05u M%+06.1f    ", xMagDataRaw.siMagZ, (double)(xAttitude.fMagHeading * RAD_TO_DEG));
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		
		sprintf(pcUartWriteBuffer, "Y%+06.1f M%+06.1f    ", (double)(xAttitude.fYaw * RAD_TO_DEG), (double)(xAttitude.fMagHeading * RAD_TO_DEG));
		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));

//                sprintf(pcUartWriteBuffer, "Y%+06.1f S%+03.1f    ", (double)(xAttitude.fYaw * RAD_TO_DEG), (double)(xGpsData.fGroundSpeed * MS_TO_KMH));
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));

//		
//		// Display the Temperature.
//		sprintf(pcUartWriteBuffer, "G%+06.1f P%+06.1f   ", (double)xGpsData.fAltitude, (double)fGetPressureAltitude());
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
//		
//		
//		
//		// First row.
//		prv_vSetCursorPosition(0, 0);
//		sprintf(pcUartWriteBuffer, "2:%u     ", prv_siRcRx2);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
//		
//		prv_vSetCursorPosition(0, 8);
//		sprintf(pcUartWriteBuffer, "3:%u     ", prv_siRcRx3);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
//		
//		
//		
//		// Second row.
//		prv_vSetCursorPosition(1, 0);
//		sprintf(pcUartWriteBuffer, "4:%u     ", prv_siRcRx4);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
//		
//		prv_vSetCursorPosition(1, 8);
//		sprintf(pcUartWriteBuffer, "5:%u     ", prv_siRcRx5);
//		ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiSLcd = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiSLcd < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
		
		// Delay for 500ms (2Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 5);
	}	
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vClearScreen
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Clear the screen of the serial LCD.
*
*******************************************************************************/
static void prv_vClearScreen(void)
{
	pcUartWriteBuffer[0] = 0xfe;
	pcUartWriteBuffer[1] = 0x01;
	ucSendToPc(pcUartWriteBuffer, 2);
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_vSetCursorPosition
*
* PARAMETERS:
* ~ ucRow		- Row on serial LCD (0 - 1).
* ~ ucColumn	- Column on serial LCD (0 - 15).
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Set the cursor position on the serial LCD.
*
*******************************************************************************/
static void prv_vSetCursorPosition(unsigned char ucRow, unsigned char ucColumn)
{
	pcUartWriteBuffer[0] = 0xfe;
	pcUartWriteBuffer[1] = 0x80 | (ucRow * 64 + ucColumn);
	ucSendToPc(pcUartWriteBuffer, 2);
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_vSetBacklight
*
* PARAMETERS:
* ~ ucBrightness	- Brightness of the backlight (0 - 100).
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Set the brightness of the backlight.
*
*******************************************************************************/
static void prv_vSetBacklight(unsigned char ucBrightness)
{
	// Change the brightness value into the range of 128-157.
	ucBrightness = (unsigned char)((unsigned int)ucBrightness * 29 / 100);
	ucBrightness += 128;
	
	// Make sure the brightness is in range.
	if (ucBrightness < 128) {
		ucBrightness = 128;
	}
	else if (ucBrightness > 157) {
		ucBrightness = 157;
	}	
	
	// Send the command to change the brightness.
	pcUartWriteBuffer[0] = 0x7c;
	pcUartWriteBuffer[1] = (char)ucBrightness;
	ucSendToPc(pcUartWriteBuffer, 2);
}	
